Imports Microsoft.DirectX.Direct3D.Geometry

Public Class MathHelpers
    ''' <summary>
    ''' Rotates a position around a specified avis.
    ''' </summary>
    ''' <param name="Axis">The point in 3D space that the Position will be rotated around.</param>
    ''' <param name="Position">The starting position you want to rotate.</param>
    ''' <param name="Rotation">The XYZ rotation value to rotate.</param>
    ''' <returns></returns>
    Public Shared Function RotateAroundAxis(ByVal Axis As Vector3, ByVal Position As Vector3, ByVal Rotation As Vector3, Optional ByVal YawPitchRoll As Boolean = True, Optional ByVal Order As AxisOrder = AxisOrder.XYZ) As Vector3
        Dim Tmp As Vector3
        Dim M As Matrix = Matrix.Identity

        Tmp = Vector3.Subtract(Position, Axis)
        M.Multiply(Matrix.Translation(Tmp))
        If YawPitchRoll Then
            M.Multiply(Matrix.RotationYawPitchRoll(Rotation.X, Rotation.Y, Rotation.Z))
        Else
            Select Case Order
                Case AxisOrder.XYZ
                    M.Multiply(Matrix.RotationX(Rotation.X))
                    M.Multiply(Matrix.RotationY(Rotation.Y))
                    M.Multiply(Matrix.RotationZ(Rotation.Z))

                Case AxisOrder.XZY
                    M.Multiply(Matrix.RotationX(Rotation.X))
                    M.Multiply(Matrix.RotationZ(Rotation.Z))
                    M.Multiply(Matrix.RotationY(Rotation.Y))

                Case AxisOrder.YXZ
                    M.Multiply(Matrix.RotationY(Rotation.Y))
                    M.Multiply(Matrix.RotationX(Rotation.X))
                    M.Multiply(Matrix.RotationZ(Rotation.Z))

                Case AxisOrder.ZYX
                    M.Multiply(Matrix.RotationZ(Rotation.Z))
                    M.Multiply(Matrix.RotationY(Rotation.Y))
                    M.Multiply(Matrix.RotationX(Rotation.X))

                Case AxisOrder.ZXY
                    M.Multiply(Matrix.RotationZ(Rotation.Z))
                    M.Multiply(Matrix.RotationX(Rotation.X))
                    M.Multiply(Matrix.RotationY(Rotation.Y))

                Case AxisOrder.YZX
                    M.Multiply(Matrix.RotationY(Rotation.Y))
                    M.Multiply(Matrix.RotationZ(Rotation.Z))
                    M.Multiply(Matrix.RotationX(Rotation.X))

            End Select
        End If

        Return New Vector3(Axis.X + M.M41, Axis.Y + M.M42, Axis.Z + M.M43)
    End Function

    Public Shared Function GetRotation(ByVal Source As Vector2, ByVal Destination As Vector2) As Single
        Dim Angle As Single
        Angle = GetAngle(Source, Destination)
        Return CorrectDegree(Source, Destination, Angle)
    End Function

    Public Shared Function GetRotation(ByVal Axis As Orientation, ByVal Source As Vector3, ByVal Destination As Vector3) As Single
        Dim Angle As Single
        Select Case Axis
            Case Orientation.XY
                Angle = GetAngle(Axis, Source, Destination)
                Return CorrectDegree(Axis, Source, Destination, Angle)

            Case Orientation.XZ
                Angle = GetAngle(Axis, Source, Destination)
                Return CorrectDegree(Axis, Source, Destination, Angle)

            Case Orientation.ZY
                Angle = GetAngle(Axis, Source, Destination)
                Return CorrectDegree(Axis, Source, Destination, Angle)
        End Select
    End Function

    Public Shared Function GetSlope(ByVal Source As Vector2, ByVal Destination As Vector2) As Single
        Return Math.Abs(Destination.Y - Source.Y) / Math.Abs(Destination.X - Source.X)
    End Function

    Public Shared Function GetSlope(ByVal Axis As Orientation, ByVal Source As Vector3, ByVal Destination As Vector3) As Single
        Select Case Axis
            Case Orientation.XY
                Return Math.Abs(Destination.Y - Source.Y) / Math.Abs(Destination.X - Source.X)

            Case Orientation.XZ
                Return Math.Abs(Destination.Z - Source.Z) / Math.Abs(Destination.X - Source.X)

            Case Orientation.ZY
                Return Math.Abs(Destination.Y - Source.Y) / Math.Abs(Destination.Z - Source.Z)
        End Select
    End Function

    Public Shared Function GetAngle(ByVal Source As Vector2, ByVal Destination As Vector2) As Single
        Dim Slope, Ang As Single

        Slope = GetSlope(Source, Destination)
        Ang = CSng(Math.Atan(Slope))
        Return Ang
    End Function

    Public Shared Function GetAngle(ByVal Axis As Orientation, ByVal Source As Vector3, ByVal Destination As Vector3) As Single
        Dim Slope, Ang As Single

        Slope = GetSlope(Axis, Source, Destination)
        Ang = CSng(Math.Atan(Slope))
        Return Ang
    End Function

    Public Shared Function CorrectDegree(ByVal Source As Vector2, ByVal Destination As Vector2, ByVal Angle As Single) As Single
        If Destination.Y < Source.Y Then
            If Destination.X < Source.X Then  ' upleft
                Return RadianToDegree(Angle) + 180
            Else                 ' upright
                Return -RadianToDegree(Angle) + 360
            End If
        Else
            If Destination.X < Source.X Then ' low left
                Return -RadianToDegree(Angle) + 180
            Else       'low right
                ' do nothing
                Return RadianToDegree(Angle)
            End If
        End If
    End Function

    Public Shared Function CorrectDegree(ByVal Axis As Orientation, ByVal Source As Vector3, ByVal Destination As Vector3, ByVal Angle As Single) As Single
        Select Case Axis
            Case Orientation.XY
                Return CorrectDegree(New Vector2(Source.X, Source.Y), New Vector2(Destination.X, Destination.Y), Angle)

            Case Orientation.XZ
                Return CorrectDegree(New Vector2(Source.X, Source.Z), New Vector2(Destination.X, Destination.Z), Angle)

            Case Orientation.ZY
                Return CorrectDegree(New Vector2(Source.Z, Source.Y), New Vector2(Destination.Z, Destination.Y), Angle)
        End Select
    End Function
End Class
